#ifndef __VSKPTTRACKER_HPP__
#define __VSKPTTRACKER_HPP__


#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <vector>
//#include "OpenCV/matwork.hpp"
#include "MatWork.hpp"
#include "ObjectDetector.hpp"


using namespace cv;
using namespace std;


class VSKptTracker
{

public:

	enum MATCH_MODE
	{
		MATCH_WITHOUT_POST, 
		MATCH_WITH_POST
	};

	VSKptTracker::VSKptTracker();

	VSKptTracker(string frameName);

	VSKptTracker(Mat inputFrame);

	int ClassInit();   // a way around for Delegating constructor which is not supported by compiler earlier than C++11
  
  
  ~VSKptTracker();
  
  
  int InitMatching(const Mat frame, vector<Point2f> *points, int size, vector<Point2f>& error);

  // initmatching with MATCH_MODE and pnts_ref
  int InitMatching(const Mat frame, Mat& prevGray_in, Mat& gray_in, vector<Point2f> *points, int size, vector<Point2f>& error, vector<Point2f>& pnts_ref_in, MATCH_MODE mode);

  // with returned pnts_ref and MATCH_MODE
  int MatchOneIter(Mat& prevGray_in, Mat& gray_in, Mat& image, vector<Point2f> *points, int size, 
							   vector<Point2f>& error, vector<Point2f>& pnts_ref_in, MATCH_MODE mode);
  
  // w/o returned pnts_ref and with MATCH_MODE
  int MatchOneIter(Mat& prevGray_in, Mat& gray_in, Mat& image, vector<Point2f> *points, int size, 
																   vector<Point2f>& error, MATCH_MODE mode);
  
  int MatchOneIter(const Mat prevGray_in, const Mat gray_in, Mat& image, vector<Point2f> *points, int size, vector<Point2f>& error);

  // overloading to return an additional var pnts_ref_in
  int MatchOneIter(const Mat prevGray_in, const Mat gray_in, Mat& image, vector<Point2f> *points, int size, 
						vector<Point2f>& error, vector<Point2f>& pnts_ref_in); 

  int OneIterPost(Mat& prevGray_in, Mat& gray_in, vector<Point2f> *points, int size, vector<Point2f>& error);
 
private:

  MATCH_MODE matchMode;
   
  int checkConsistency(vector<Point2f> points_a, vector<Point2f> points_b, vector<Point2f>& pnts_ref);
  
  int printfDebuggingVars(vector<Point2f> pnts_ref, vector<Point2f> pnts_0, vector<Point2f> pnts_1, 
			vector<Point2f> pnts_2, vector<Point2f> error, vector<uchar> status);
  
  // SIFT Matcher vars
  vector<DMatch> enh_good_matches;
  vector<KeyPoint> keypoints_img, keypoints_ref;
  Mat descriptor_img, descriptor_ref;
  int vote;
  Mat ref_img;
  Mat matched_pnts_ref, matched_pnts_img;
  
  SIFT SIFTdect;
  TermCriteria termcrit;
  Size subPixWinSize, winSize;

  // Optical flow & VS vars
  vector<DMatch> matches;  
  
  Point2f point;
  bool addRemovePt;
  
  int MAX_COUNT; 
  int needToInit; 
  //bool nightMode;
  
  Mat gray, prevGray, image;
  vector<Point2f> points[3];
  
  vector<uchar> status;
  vector<float> err;

  vector<Point2f> ds;      // ds = s(k) - s(k-1); at every iteration
  vector<Point2f> error;   // error = s_star - s(k); at every iteration
  vector<Point2f> pnts_ref, pnts_ref_tmp; 
  Point2f p;

  // flag for optical flow
  int flag;
  
  //int init_flag;
  
  Point2i pr, pi;
  Point2f prf, pif;
  
  
};

#endif 